#include "MainAppC.h"
#include "Package.h"

#define MAX_NR_SEND 1
#define MSG_TO_SEND 5000
#define POST_TIMEOUT 5000
#define SEND_PERIODIC 50
#define POWER_SEND 3
#define POWER_RECEIVE 3
#define POWER_PASSIVE 3
#define NR_EXPECTED_IN_ROW 5

#define ID_A 1
#define ID_B 2
#define ID_C 3
#define ID_ALL AM_BROADCAST_ADDR //65535

module StatesC {
	provides interface StateInterface;
	provides interface StateDataInterface as Data;
	uses interface SendControl;
	uses interface Timer<TMilli> as EventATimer;
	uses interface Timer<TMilli> as PostTimer;
	uses interface PackageIDFifo as Fifo;
	uses interface PackageIDFifo as ReceiveMissing;
	uses interface PackageIDFifo as PassiveQueue;
	uses interface Data as DataControl;
	uses interface SplitControl as AMControl;
}
implementation {
	int counter = 1;
	int expected = 1; //The next expected package to receive.
	int retransCounter = 0;
	Mode currentMode;
	int passiveCounter = 0;
	int counterTotalSend = 0;
	int numberOfExpectedInRow = 0;
	bool useMoteB = FALSE;
	int arqReceivedFromC = 0;

	void requestRetransmit(Package * data) {
		int count, numberOfMissings = call ReceiveMissing.size();
		int16_t missings[numberOfMissings];
		Package ARQ;
		retransCounter++;

		if(numberOfMissings > DATA_SIZE - 2) 
			numberOfMissings = DATA_SIZE - 2;

		call ReceiveMissing.peek(missings, numberOfMissings);

		ARQ.data[0] = data->data[0];
		ARQ.data[1] = numberOfMissings;

		for(count = 0; count < numberOfMissings; count++) {
			call ReceiveMissing.enqueue(missings[count]);
			ARQ.data[count + 2] = missings[count];
		}

		ARQ.arq = TRUE;
		ARQ.ack = FALSE;
		ARQ.moteID = ID_C;
		if(useMoteB == TRUE) 
			call SendControl.sendNewPackage(&ARQ, POWER_RECEIVE, ID_B);
		else 
			call SendControl.sendNewPackage(&ARQ, POWER_SEND, ID_ALL);
	}

	async command void StateInterface.ChangeState(Mode modes) {
		switch(modes) {
			case Passive : passiveCounter = 0;
			arqReceivedFromC = 0;
			call EventATimer.stop();
			break;
			case Send : counter = 1;
			counterTotalSend = 0;
			call Fifo.trashAll();
			call EventATimer.startPeriodic(SEND_PERIODIC);
			break;
			case Receive : //Reset
			expected = 1;
			numberOfExpectedInRow = 0;
			useMoteB = FALSE;
			call DataControl.Text2("Queue status:", call ReceiveMissing.status());
			call ReceiveMissing.trashAll();
			call DataControl.Text1("Ready to receive again.");
			retransCounter = 0;
			call EventATimer.stop();
			break;
		}

		currentMode = modes;
	}

	event void EventATimer.fired() {
		Package message;

		int numberOfMissings = call Fifo.size();
		int i, size;

		switch(currentMode) {
			case Passive : if(call PassiveQueue.size() == 0) {
				call EventATimer.stop();
				break;
			}

			message.ack = FALSE;
			message.arq = FALSE;
			message.data[0] = MSG_TO_SEND;
			message.data[1] = 0;
			message.moteID = ID_B;

			message.id = call PassiveQueue.dequeue();
			passiveCounter++;
			call DataControl.Text2("passiveCounter: ", passiveCounter);

			call SendControl.sendNewPackage(&message, POWER_PASSIVE, ID_C);

			break;

			case Send : // Initialize data with number of message to send
			for(i = 0; i < DATA_SIZE; i++) 
				message.data[i] = MSG_TO_SEND;

			message.id = 0;
			message.moteID = ID_A;
			message.arq = FALSE;
			message.ack = FALSE;

			if(call Fifo.size() == 0 && counter <= MSG_TO_SEND) {
				call Fifo.enqueue(counter++);
				numberOfMissings++;
			}

			size = call Fifo.size();

			if(size > MAX_NR_SEND) 
				size = MAX_NR_SEND;

			for(i = 0; i < size; i++) {
				message.id = call Fifo.dequeue();
				call SendControl.sendNewPackage(&message, POWER_SEND, ID_ALL);
				counterTotalSend++;
				call DataControl.Stream(&message);
			}

			if(counter >= MSG_TO_SEND && call Fifo.size() == 0 && ! call PostTimer
					.isRunning()) {
				call PostTimer.startPeriodic(POST_TIMEOUT);
				call DataControl.Text1("PostTimer started");
			}

			else 
				if(counter >= MSG_TO_SEND && call Fifo.size() == 0) 
				counter = MSG_TO_SEND;

			break;
		}
	}

	event void PostTimer.fired() {
		if(call Fifo.size() == 0) {
			call EventATimer.stop();
			call PostTimer.stop();
			call DataControl.Text1("Finished !");
			call DataControl.Text2("Total Transmitted: ", counterTotalSend);
		}
	}
	async command void Data.DataReceived(Package * data) {
		bool needRetransmit = TRUE;
		switch(currentMode) {
			case Passive : if(data->arq == TRUE) {
				int iCount;
				arqReceivedFromC++;
				call DataControl.Text2("#ARQ received: ", arqReceivedFromC);

				for(iCount = 0; iCount < data->data[1]; iCount++) {
					call PassiveQueue.enqueue(data->data[iCount + 2]);

				}
				call EventATimer.startPeriodic(SEND_PERIODIC);
			}

			break;

			case Receive : 	//Expected package received
			if(data->id == expected) {
				expected++;
				numberOfExpectedInRow++;
				call DataControl.Stream(data);

				if(numberOfExpectedInRow == NR_EXPECTED_IN_ROW) {
					if(call ReceiveMissing.size() != 0) {
						requestRetransmit(data);
						needRetransmit = FALSE;
					}
					numberOfExpectedInRow = 0;
				}
			}
			//Received package was not missing - then add it to missings and send ARQs.
			else 
				if(data->id > expected) {
				int missingCounter;
				numberOfExpectedInRow = 0;

				call DataControl.Stream(data);

				for(missingCounter = expected; missingCounter < data->id; missingCounter++) {
					call ReceiveMissing.enqueue(missingCounter);
				}

				expected = data->id + 1;

				requestRetransmit(data);
				needRetransmit = FALSE;
			}
			//Received package was missing
			else {
				int count = call ReceiveMissing.remove(data->id);
				if(count > 0) {
					if(data->moteID == ID_B) 
						useMoteB = TRUE;

					call DataControl.Stream(data);
					data->rssi = count;
					call DataControl.Stream(data);
					needRetransmit = FALSE;
					numberOfExpectedInRow = 0;
				}
			}

			if(call ReceiveMissing.size() == 0 && expected > data->data[0]) {
				Package ackPtr;
				ackPtr.ack = TRUE;
				ackPtr.arq = 0;
				ackPtr.id = 0;
				ackPtr.moteID = ID_C;
				ackPtr.data[0] = data->data[0];
				call SendControl.sendNewPackage(&ackPtr, POWER_SEND, ID_A);
				call DataControl.Text1("Transfer completed.");
				call DataControl.Text2("Total Retransmits Request:", retransCounter);
				call DataControl.Text2("Total packages removed", call ReceiveMissing
						.status());
			}
			else 
				if(expected > data->data[0]&& needRetransmit == TRUE) {
				requestRetransmit(data);
			}
			break;

			case Send : 	if(data->arq == TRUE) {
				int forCounter;

				for(forCounter = 0; forCounter < data->data[1]; forCounter++) {
					call DataControl.Text2("Request id", data->data[forCounter + 2]);
					call Fifo.enqueue(data->data[forCounter + 2]);
				}
			}
			else 
				if(data->ack == TRUE) {
				call PostTimer.stop();
				call EventATimer.stop();
				call DataControl.Text1("Finished with Ack!");
				call DataControl.Text2("Total Transmitted: ", counterTotalSend);
			}

			break;
		}
	}

	async command void StateInterface.InitStates() {
		call AMControl.start();
	}

	async command void StateInterface.TeardownStates() {
		call AMControl.stop();
	}

	event void AMControl.startDone(error_t err) {
		if(err == SUCCESS) {
			call DataControl.Text1("TelosB has been reset");
		}
		else {
			call AMControl.start();
		}
	}

	event void AMControl.stopDone(error_t err) {

	}
}